﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Intersection
{
    class Intersection
    {
        // 原始观测数据
        private ObsData obsData;

        // 左右像片旋转矩阵
        private double[][] R_left, R_right;

        public Intersection() { this.obsData = null; }
        public Intersection(ObsData obsData) { this.obsData = obsData; }

        private double[][] ComputeRotationMatrix(ImageParam image)
        {
            double a1 = Math.Cos(image.phi) * Math.Cos(image.kappa) - Math.Cos(image.phi) * Math.Sin(image.omega) * Math.Sin(image.kappa);
            double a2 = -Math.Cos(image.phi) * Math.Sin(image.kappa) - Math.Sin(image.phi) * Math.Sin(image.omega) * Math.Sin(image.kappa);
            double a3 = -Math.Sin(image.phi) * Math.Cos(image.kappa);

            double b1 = Math.Cos(image.omega) * Math.Sin(image.kappa);
            double b2 = Math.Cos(image.omega) * Math.Cos(image.kappa);
            double b3 = -Math.Sin(image.omega);

            double c1 = Math.Sin(image.phi) * Math.Cos(image.kappa) + Math.Cos(image.phi) * Math.Sin(image.omega) * Math.Sin(image.kappa);
            double c2 = -Math.Sin(image.omega) * Math.Cos(image.kappa) + Math.Cos(image.phi) * Math.Sin(image.omega) * Math.Sin(image.kappa);
            double c3 = Math.Cos(image.phi) * Math.Cos(image.omega);

            double[][] R = { new double[]{ a1, a2, a3 }, new double[] { b1, b2, b3 }, new double[] { c1, c2, c3 } };

            return R;
        }

        private void ComputeAuxImageCoords(out List<List<double>> left, out List<List<double>> right)
        {
            left = new List<List<double>>();
            right = new List<List<double>>();

            R_left = ComputeRotationMatrix(obsData.Img_left);
            R_right = ComputeRotationMatrix(obsData.Img_right);

            for (int i = 0; i < obsData.Data.Count; i++)
            {
                CorrPoints pts = obsData.Data.ElementAt(i);

                double u1 = R_left[0][0] * pts.x1 + R_left[0][1] * pts.y1 - R_left[0][2] * pts.f1;
                double v1 = R_left[1][0] * pts.x1 + R_left[1][1] * pts.y1 - R_left[1][2] * pts.f1;
                double w1 = R_left[2][0] * pts.x1 + R_left[2][1] * pts.y1 - R_left[2][2] * pts.f1;

                double u2 = R_right[0][0] * pts.x2 + R_right[0][1] * pts.y2 - R_right[0][2] * pts.f2;
                double v2 = R_right[1][0] * pts.x2 + R_right[1][1] * pts.y2 - R_right[1][2] * pts.f2;
                double w2 = R_right[2][0] * pts.x2 + R_right[2][1] * pts.y2 - R_right[2][2] * pts.f2;

                left.Add(new List<double> { u1, v1, w1 });
                right.Add(new List<double> { u2, v2, w2 });
            }
        }

        private void ComputeProjectCoeff(List<List<double>> left, List<List<double>> right, out List<List<double>> N)
        {
            N = new List<List<double>>();

            double BU = obsData.Img_right.Xs - obsData.Img_left.Xs;
            double BV = obsData.Img_right.Ys - obsData.Img_left.Ys;
            double BW = obsData.Img_right.Zs - obsData.Img_left.Zs;

            for (int i = 0; i < left.Count; i++)
            {
                double N1 = (BV * right[i][2] - BW * right[i][0]) / (left[i][0] * right[i][2]- right[i][0] * left[i][2]);
                double N2 = (BV * left[i][2] - BW * left[i][0]) / (left[i][0] * right[i][2] - right[i][0] * left[i][2]);

                N.Add(new List<double> { N1, N2 });
            }
        }

        private void ComputeRealCoord(List<List<double>> left, List<List<double>> right, List<List<double>> N, out List<List<double>> coords)
        {
            coords = new List<List<double>>();

            for (int i = 0; i < N.Count; i++)
            {
                double X = obsData.Img_left.Xs + N[i][0] * left[i][0];
                double Y = 0.5 * (obsData.Img_left.Ys + N[i][0] * left[i][1] + obsData.Img_right.Ys + N[i][1] * right[i][1]);
                double Z = obsData.Img_left.Zs + N[i][0] * left[i][2];

                coords.Add(new List<double> { X, Y, Z });
            }
        }


        public void Compute()
        {
            List<List<double>> left, right, N, realCoord;
            ComputeAuxImageCoords(out left, out right);
            ComputeProjectCoeff(left, right, out N);
            ComputeRealCoord(left, right, N, out realCoord);
        }
    }
}
